perm filename MOTOR.SAI[AL,HE] blob
sn#266854 filedate 1977-03-07 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00011 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 PROGRAM TO DRIVE A SINGLE JOINT ON THE ARMS USING THE PDP11
C00005 00003 ⊃ CALIBRATION DATA
C00007 00004 SIMPLE PROCEDURE AXIS(INTEGER I0,IM,X0,Y0,NX,NY,MIN,MAXSTRING XUNIT,YUNIT
C00010 00005 ⊃ START OF RUN TIME CODE
C00012 00006 ⊃ READ IN THE JOINT, MAXIMUM AMPLITUDE, FUNCTION CYCLE TIME, AND DURATION
C00015 00007 ⊃ SEND COMMAND TO THE ELF. IF RUN IS NOT ZERO, SOMETHINGS WRONG WITH THE
C00017 00008 ⊃ READ IN THE LIMITS OF THE INTERVAL TO BE DISPLAYED
C00020 00009 ⊃ DETERMINE THE RELATIVE ADDRESS OF THE START TIME OF THE INTERVAL TO
C00023 00010 ⊃ DISPLAY THE DATA SEND FROM THE ELF
C00026 00011 ⊃ DISPLAY THE THEORETICAL JOINT ANGLE PLOT AND SLEEP FOR A WHILE
C00029 ENDMK
C⊗;
COMMENT PROGRAM TO DRIVE A SINGLE JOINT ON THE ARMS USING THE PDP11;
BEGIN "MOTOR"
REQUIRE "TLKELF.FAI" LOAD_MODULE;
REQUIRE "POTRED" LOAD_MODULE;
DEFINE ⊃="COMMENT ",CRLF="('15&'12)",FUNCT="1",HEAD="2",CR="'15";
INTEGER ARM,DCOFF,AMP,CYCLE,DURA,DUM,GRF,JOINT,I,RUN,MASTER,K,J,TP;
INTEGER XGRID,YGRID,QX,QY,ST,FT,BP,NK,NJ;
INTEGER LL,UL,POG,MODULUS,GDISP_INIT;
LABEL ARMLP,AMPLP,RUNELF,MDONE;
STRING WARM,COM1,COM2,SARMJT,COM3;
STRING DISCM;
STRING ANS,LINED,ANS2,GANS;
STRING DANS,RELD;
REAL DTIME,CVEL,PVEL,DVEL,F;
INTEGER ARRAY STIME[1:20],FTIME[0:20],SWORD[1:20],GDISP[0:14];
INTEGER ARRAY DATA[1:3002],MOTOR[1:300],ANGLE[1:300];
INTEGER ARRAY DISPLY[1:'3000];
PRELOAD_WITH "RELATIVE JOINT ANGLE IN 1/100 DEGREE",
"JOINT VELOCITY IN 1/10000 DEG/MSEC";
STRING ARRAY TITLE[0:1];
REAL INERTIA,DAMP,DRIVE;
INTEGER SAVEPT,III;
INTEGER ARRAY POTS[1:4];
REQUIRE "DPYSUB.HDR[1,PDQ]" SOURCE_FILE;
EXTERNAL INTEGER PROCEDURE TLKELF( INTEGER ARM,JOINT,DCOFF,AMP,CYCLE,DURA,
TP,MASTER,GRF;INTEGER ARRAY DATA);
EXTERNAL PROCEDURE POTRED( INTEGER ARRAY NX );
⊃ CALIBRATION DATA;
⊃ POT SCALE FACTOR;
PRELOAD_WITH -8.70827,6.68151,-.915081,9.61282,-6.68151,-8.628954
,-.135086;
REAL ARRAY SCALE[1:7];
⊃ TACHOMETER SCALE FACTOR;
PRELOAD_WITH .93541,0.7585,-.25151,6.7156,-.72031,0.0,0.0;
REAL ARRAY VSCALE[1:7];
⊃ DAC VALUE TO MOTOR TORQUE OUTPUT SCALE FACTOR;
PRELOAD_WITH +1,+1,-1,+1,-1,+1,+1;
INTEGER ARRAY MOTSNS[1:7];
PRELOAD_WITH -68.45, 186.8, -2.3, -5.897, 7.695, -8.0487, -9.12;
REAL ARRAY MSCALE[1:7];
⊃ NON-SYMMETRIC OFFSET IN DAC OUTPUT;
PRELOAD_WITH -1,0,0,0,0,0,0;
INTEGER ARRAY OFFSET[1:7];
⊃ ESTIMATED JOINT FRICTION IN OZ-IN.;
PRELOAD_WITH 1153.0, 1188.0, 18.0, 1.0, 110.8, 88.47, 56.0;
REAL ARRAY FRICT[1:7];
⊃ ESTIMATED JOINT INERTIA;
PRELOAD_WITH 3.867@8, 4.55@8, 3.85@5, 1.04@7, 1.21@7, 8.87@7, 1.590@6;
REAL ARRAY ISCALE[1:7];
⊃ ESTIMATED JOINT DAMPING TERM;
PRELOAD_WITH 0.0,0.0,0.0,0.0,0.0,0.0,0.0;
REAL ARRAY DSCALE[1:7];
⊃ CONVERSION FACTOR FROM RADIANS TO DEGREES IF NEEDED;
PRELOAD_WITH 57.29578,57.29578,1.0,57.29578,57.29578,57.29578,1.0;
REAL ARRAY CONV[1:7];
SIMPLE PROCEDURE AXIS(INTEGER I0,IM,X0,Y0,NX,NY,MIN,MAX;STRING XUNIT,YUNIT;
REFERENCE INTEGER XGRID,YGRID,QX,QY);
BEGIN INTEGER WID,DIG;
REAL DX,DY;
GETFORMAT(WID,DIG);SETFORMAT(1,0);
DX←NX/(IM-I0); DY←NY/(MAX-MIN); ⊃ Scale the axes;
XGRID←MKSCALE(X0,Y0,DX,0,100,I0,IM,XUNIT);
YGRID←MKSCALE(X0,Y0,0,DY,40,MIN,MAX,YUNIT);⊃ Draw the axes;
QY←YGRID*DY;QX←XGRID*DX;
SETFORMAT(WID,DIG);
RETURN;
END;
SIMPLE PROCEDURE AXIS2(INTEGER X0,Y0,NY,MIN,MAX;STRING YUNIT;
REFERENCE INTEGER YGRID,QY);
BEGIN INTEGER WID,DIG;
REAL DY;
GETFORMAT(WID,DIG);SETFORMAT(1,0);
DY←NY/(MAX-MIN); ⊃ Scale the axes;
YGRID←MKSCALE(X0,Y0,0,DY,40,MIN,MAX,YUNIT);⊃ Draw the axes;
QY←YGRID*DY;
SETFORMAT(WID,DIG);
RETURN;
END;
SIMPLE PROCEDURE APLOT
(INTEGER ARRAY A;INTEGER I0,IM,X0,Y0,MIN,MAX,XGRID,YGRID,QX,QY);
⊃ Graphs array A with subscripts in the range I0 to IM, with origin
at X0,Y0 and dimensions NX and NY. The axes are labelled XUNIT and
YUNIT. If I0=IM then the actual array bounds are used for I0 and IM.
NX=0 allows one to overlay a graph on a previous graph, without
relabelling or rescaling the axes;
BEGIN INTEGER I,J,X,AI,C,M,WID,DIG;
LABEL L1;
GETFORMAT(WID,DIG);SETFORMAT(1,0);
AIVECT(X0,QY*A[I0] DIV YGRID-QY*MIN DIV YGRID+Y0);
J←I0;
DO BEGIN
M←A[J+1]-A[J];
C←A[J]-M*J;
FOR I←J+2 STEP 1 UNTIL IM DO
IF A[I]≠I*M+C THEN BEGIN I←I-1;GO TO L1 END;
I←IM;
L1: RVECT(QX*I DIV XGRID-QX*J DIV XGRID,QY*A[I] DIV YGRID-QY*A[J] DIV YGRID);
J←I;
END
UNTIL J=IM;
SETFORMAT(WID,DIG);
RETURN;
END;
⊃ START OF RUN TIME CODE;
MASTER←'11711;
SETBREAK(HEAD,"ABCDEFGHIJKLMNOPQRSTUVWXYZ?"&'12&'15,NULL,"INR");
SETBREAK(FUNCT,"ABCDEFGHIJKLMNOPQRSTUVWXYZ?0123456789",NULL,"XNR");
LINED←"7 0 20 100 2000";
DANS←"D 1 2000";
COM3←" ";
TP←0;
OUTSTR(CRLF&CRLF&"ARM JOINT MOTOR DRIVING PROGRAM"&CRLF);
⊃ CHECK WHETHER RUNNING ON A III OR A DATA DISK DISPLAY;
START_CODE
DEFINE PPINFO= "'702240000000";
PPINFO @DATA;
MOVE 1,DATA;
HLRZ 2(1);
SETZM III;
TRNN '100000;
AOS III;
END;
⊃ READ IN THE ARM TO BE USED AND CHECK IF GRAPHICS TO BE DONE;
ARMLP: OUTSTR("Yellow OR Blue arm (Y,B) ? ");
ANS←INCHRW; CLRBUF;
IF EQU("Y",ANS) THEN BEGIN
WARM←"YELLOW ";
ARM←0;
END
ELSE IF EQU("B",ANS) THEN BEGIN
WARM←"BLUE ";
ARM←1;
END
ELSE GOTO MDONE;
WARM← WARM&"ARM ";
OUTSTR(CRLF&"Display dynamic response (Y,N) ? ");
ANS←INCHRW; CLRBUF; OUTSTR(CRLF);
IF ANS="Y" THEN BEGIN
GRF←1;
OUTSTR("Read Pot or Tach (P,T) ? ");
ANS←INCHRW; CLRBUF; OUTSTR(CRLF);
IF ANS="T" THEN TP←1 ELSE TP←0;
END
ELSE GRF←TP←0;
⊃ READ IN THE JOINT, MAXIMUM AMPLITUDE, FUNCTION CYCLE TIME, AND DURATION;
AMPLP: OUTSTR("Jt,DC,Sin_Amp,Sin_Per,Duration "
&CRLF);
LODED(LINED&CR);
ANS2←ANS←INCHWL;
SCAN(ANS2,HEAD,DUM);
ANS2←SCAN(ANS2,FUNCT,DUM);
IF EQU(ANS2,"?") THEN BEGIN
OUTSTR(CRLF&"The motor drive parameters are as follows:"&CRLF&
" Jt: Selected joint, 1-7"&CRLF&
" DC: DC motor drive in DAC units, -70 TO +70"&CRLF&
" Sin_Amp: Sin. drive amplitude in DAC units, -128 TO +128"
&CRLF&" Sin_Per: Sinusoidal period in MSEC, 0 TO 32000"&CRLF&
" Duration: Motor run time in MSEC, 0 TO 32000"&CRLF&CRLF);
GOTO AMPLP;
END;
⊃ CHECK FOR DRIVE PARAMETERS WITHIN RANGE;
LINED←ANS;
JOINT←INTSCAN(ANS,DUM);
IF((JOINT<1)∨(JOINT>7)) THEN BEGIN
OUTSTR("ILLEGAL JOINT SPECIFICATION"&CRLF);
GO TO ARMLP;
END;
SARMJT← WARM&"JOINT # "&CVS(JOINT);
DCOFF←INTSCAN(ANS,DUM)*MOTSNS[JOINT];
AMP←INTSCAN(ANS,DUM)*MOTSNS[JOINT];
IF((DCOFF<-70)∨(DCOFF>70)) THEN BEGIN
OUTSTR("DC OFFSET OUT OF BOUND"&CRLF);
GO TO ARMLP;
END;
CYCLE←INTSCAN(ANS,DUM);
IF((CYCLE<0)∨(CYCLE>32000)) THEN BEGIN
OUTSTR("ILLEGAL SINUSOIDAL PERIOD"&CRLF);
GO TO ARMLP;
END;
IF CYCLE=0 THEN BEGIN
CYCLE←1;
AMP←0;
END;
DURA←INTSCAN(ANS,DUM);
IF((DURA<1)∨(DURA>32000)) THEN BEGIN
OUTSTR("ILLEGAL DRIVE DURATION"&CRLF);
GO TO ARMLP;
END;
⊃ SEND COMMAND TO THE ELF. IF RUN IS NOT ZERO, SOMETHINGS WRONG WITH THE
ELF;
RUNELF: RUN←TLKELF(ARM,JOINT,DCOFF,AMP,CYCLE,DURA,TP,MASTER,GRF,DATA);
IF RUN≠0 THEN GOTO AMPLP;
⊃ CHECK IF DYNAMIC RESPONSE TO BE PLOTTED. RETURN TO COMMAND INPUT WAIT
LOOP IF GRF ZERO.;
IF GRF=0 THEN GOTO AMPLP ELSE BEGIN
OUTSTR("Do you want graphics now (Y,N)? ");
GANS←INCHRW; CLRBUF;
IF ¬EQU("Y",GANS) THEN GOTO AMPLP;
END;
⊃ SCAN DATA TO DETERMINE INTERVALS OF DATA THAT WERE COLLECTED. START TIMES
OF INTERVAL IN STIME, FINAL TIMES IN FTIME, DATA WORD CORRESPONDING TO START
TIME IN SWORD.;
I←1;
J←-10;
K←0;
WHILE DATA[I]≥0 DO BEGIN
IF J≠DATA[I] THEN BEGIN
FTIME[K]←J-1;
J←DATA[I];
K←K+1;
STIME[K]←J;
SWORD[K]←I;
END;
J←J+1;
I←I+1;
END;
FTIME[K]←J-1;
STIME[K+1]←1234567;
⊃ PRINT OUT THE INTERVALS OF TIME FOR WHICH WE HAVE DATA;
IF DATA[1]=1234567 THEN BEGIN
OUTSTR("NO DATA SENT FROM ELF"&CRLF);
GO TO AMPLP;
END;
OUTSTR(CRLF&"Data collection period(s):"&CRLF);
SETFORMAT(5,0);
I←1;
WHILE STIME[I]≠1234567 DO BEGIN
OUTSTR(" "&CVS(STIME[I])&" MSEC. TO "&
CVS(FTIME[I])&" MSEC."&CRLF);
I←I+1;
END;
⊃ READ IN THE LIMITS OF THE INTERVAL TO BE DISPLAYED;
OUTSTR(CRLF&"Type interval of data to be displayed"&CRLF&CRLF);
SETFORMAT(0,0);
WHILE TRUE DO BEGIN
LABEL BOTTOM,LPEND;
OUTSTR("Function, Start time, End time:"&CRLF);
LODED(DANS&CR);
ANS←RELD←INCHWL;
SCAN(ANS,HEAD,DUM);
DISCM←SCAN(ANS,FUNCT,DUM);
⊃ "E" TO TERMINATE DISPLAY MODE, "D" TO DISPLAY, "P" OUTPUT XGP FILE;
IF EQU(DISCM,"?") THEN BEGIN
OUTSTR(CRLF&"The display commands are as follows:"&CRLF&
" A-Compute joint acceleration"&CRLF&
" C-Modify comment line"&CRLF&
" D-Display motor drive and joint angles "&CRLF&
" E-Terminate display mode"&CRLF&
" F-Change sense of joint angle displacement"&CRLF&
" P-Output display buffer to disk"&CRLF&
" ?-Explain possible commands"&CRLF&CRLF);
GOTO LPEND;
END;
IF EQU(DISCM,"E") THEN BEGIN
OUTSTR(CRLF);
GOTO AMPLP;
END;
⊃ MODIFY COMMENT;
IF EQU(DISCM,"C") THEN BEGIN
OUTSTR("TYPE IN A NEW COMMENT"&CRLF);
LODED(COM3&CR);
COM3←INCHWL & " ";
GOTO LPEND;
END;
⊃ IF "D","F",OR "A" MODE, GET LOWER AND UPPER LIMITS OF TIME TO BE USED;
IF EQU(DISCM,"D")∨EQU(DISCM,"F")∨EQU(DISCM,"A") THEN BEGIN
IF EQU(DISCM,"F") THEN F←-1.0 ELSE F←1.0;
LL←INTSCAN(ANS,DUM);
UL←INTSCAN(ANS,DUM);
IF LL≥UL THEN BEGIN
OUTSTR("ILLEGAL LIMITS"&CRLF);
GOTO BOTTOM;
END;
⊃ DETERMINE THE RELATIVE ADDRESS OF THE START TIME OF THE INTERVAL TO
BE DISPLAYED WITH RESPECT TO THE START OF THE DATA BUFFER, ST.;
I←0;
WHILE (STIME[I+1]≤LL)∧(STIME[I+1]≠1234567) DO I←I+1;
IF LL>FTIME[I] THEN BEGIN
I←I+1;
IF STIME[I]≠1234567 THEN LL←STIME[I]
ELSE BEGIN
OUTSTR("ILLEGAL START TIME"&CRLF);
GOTO BOTTOM;
END;
END;
ST←SWORD[I]+LL-STIME[I]+1000;
⊃ NOW DETERMINE THE RELATIVE ADDRESS OF THE FINAL TIME, FT.;
J←I;
WHILE (FTIME[J]<UL) DO IF STIME[J+1]<UL
THEN J←J+1 ELSE UL←FTIME[J];
FT←SWORD[J]+UL-STIME[J]+1000;
IF J≠I THEN OUTSTR("⊗⊗⊗ "&CVS(J-I)&" GAP(S) IN DATA⊗⊗⊗"&CRLF);
⊃ DETERMINE THE STEP FACTOR TO BE USED IN PICKING OUT WHAT DATA IS TO
BE DISPLAYED, MODULUS.;
MODULUS← IF (FT-ST)≤100 THEN 1 ELSE (FT-ST)%100+1;
⊃ INITIALIZE DISPLAY ROUTINES.;
DPYCLR;
POG←GETPOG;
DPYSET(DISPLY);
COM1←IF F<0.0 THEN SARMJT&", POT DATA FLIPPED"
ELSE SARMJT;
COM2←"START TIME = "&CVS(LL)&" FINAL TIME = "&CVS(UL)&
" WITH "&CVS(J-I)&" GAP(S) IN DATA";
⊃ TRANSFER THE DATA OVER TO THE DISPLAY BUFFERS AND STORE THE JOINT
ANGLE AS A CHANGE IN ANGLE FROM THE INITIAL READING. AT THE SAME
TIME, DETERMINE THE MAXIMUM AND MINIMUM VALUES FOR EACH ARRAY.;
BP←0;
K←1;
J←1;
FOR I←ST STEP MODULUS UNTIL FT DO BEGIN
BP←BP+1;
MOTOR[BP]←DATA[I]*MOTSNS[JOINT];
IF ABS(DATA[I])>K THEN K←ABS(DATA[I]);
CASE TP OF BEGIN
ANGLE[BP]←F*SCALE[JOINT]
*(DATA[I+1000]-DATA[ST+1000]);
ANGLE[BP]←F*VSCALE[JOINT]*(DATA[I+1000]);
END;
IF ABS(ANGLE[BP])>J THEN J←ABS(ANGLE[BP]);
END;
⊃ DISPLAY THE DATA SEND FROM THE ELF;
AXIS2(-400,-450,800,-K,K,"DRIVE",YGRID,QY);
APLOT(MOTOR,1,BP,-300,-450,-K,K,XGRID,YGRID,QX,QY);
AXIS(1,BP,-300,-450,700,800,-J,J,"T/"&CVS(MODULUS),
TITLE[TP],XGRID,YGRID,QX,QY);
APLOT(ANGLE,1,BP,-300,-450,-J,J,XGRID,YGRID,QX,QY);
DPYBIG(2);
AIVECT(-400,475);
DPYSST(COM1);
AIVECT(-400,450);
DPYSST(COM2);
AIVECT(-400,425);
DPYSST(COM3);
SAVEPT←DPYPTR;
DPYOUT(POG);
IF III≠0 THEN BEGIN
QUICK_CODE -8304460352; END; ⊃ DPYPOS -1100;
END;
⊃ "A" COMMAND, BEGIN INTERACTIVE GRAPHICS;
IF EQU(DISCM,"A") THEN BEGIN
WHILE INCHRS<0 DO BEGIN "A"
POTRED(POTS);
INERTIA←ISCALE[JOINT]*2.0↑(POTS[1]/2048);
DAMP←DSCALE[JOINT]*2.0↑(POTS[2]/700);
ANGLE[1]←0;
K←1;
PVEL←0.0;
FOR I←ST STEP MODULUS UNTIL FT-MODULUS DO BEGIN
K←K+1;
DTIME←(DATA[I+MODULUS-1000]-DATA[I-1000]);
DVEL←(MSCALE[JOINT]*(DATA[I]+OFFSET[JOINT])
-(DAMP*PVEL)-FRICT[JOINT]*
(PVEL/ABS(PVEL)))*(CONV[JOINT]/INERTIA)*DTIME;
CVEL←PVEL+DVEL;
CASE TP OF BEGIN
BEGIN
ANGLE[K]←ANGLE[K-1]+(PVEL+0.5*DVEL)*
DTIME*100.0;
END;
BEGIN
ANGLE[K]←CVEL*10000.0;
END;
END;
PVEL←CVEL;
END;
⊃ DISPLAY THE THEORETICAL JOINT ANGLE PLOT AND SLEEP FOR A WHILE;
DPYPTR←SAVEPT;
APLOT(ANGLE,1,K,-300,-450,-J,J,XGRID,YGRID,QX,QY);
AIVECT(-400,425);
SETFORMAT(9,4);
DPYSST("INERTIA ="&CVE(INERTIA)&", DAMP ="&CVE(DAMP));
DPYOUT(POG);
START_CODE
DEFINE SLEEP = "'47100000031";
MOVEI 2,1;
SLEEP ;
END;
END "A";
INCHWL;
END;
⊃ TERMINATE GRAPHICS MODE;
INCHWL;
DPYCLR;
GOTO BOTTOM;
END;
⊃ IF P COMMAND, WRITE OUT A DISK FILE CONTAINING THE DISPLAY BUFFER.;
IF EQU(DISCM,"P") THEN BEGIN
STRING FILNAM;
INTEGER FLG,CHN;
CHN←14;
OPEN(CHN,"DSK",8,0,3,0,0,0);
DO BEGIN
OUTSTR("PLOT FILE = ");
FILNAM←INCHWL;
ENTER(CHN,FILNAM&".PLT",FLG);
END UNTIL ¬FLG;
ARRYOUT(CHN,DISPLY[1],DISPLY[2]);
CLOSE(CHN);
RELEASE(CHN);
GOTO LPEND;
END;
⊃ ONLY COMES HERE IS ILLEGAL INSTRUCTION.;
OUTSTR("ILLEGAL FUNCTION"&CRLF);
BOTTOM: DANS←RELD;
LPEND: END;
MDONE: END